%% Script to Process and Plot data from test with exo and different network architecture
clc
clear variables
close all

addpath("Functions\")

ChooseData = [1,2,3];
% 1 = exo only
% 2 = transparent
% 3 = 6G emulator

%% Data Open (do not modify)
pathData = '..\01 - Data\';
FileDataNames = {'ExoOnly';'TransparentNet';'6GEmulator'};
for dataIdx = 1:length(ChooseData)
    if ChooseData(dataIdx) == 1 % exo only
        filename_ML=[pathData,FileDataNames{ChooseData(dataIdx)},'.bin'];
        file_size =  38;
        fid = fopen(filename_ML,'r','s');
        dataML = fread(fid, [file_size inf],'double');
        fclose(fid);
        dataFPGA = [];
    else
        % FPGA (LL) File
        filename_FPGA=[pathData,FileDataNames{ChooseData(dataIdx)},'_LLC.bin'];
        file_size =  18;
        fid = fopen(filename_FPGA,'r','s');
        dataFPGA = fread(fid, [file_size inf],'double');
        fclose(fid);

        % ML File
        filename_ML=[pathData,FileDataNames{ChooseData(dataIdx)},'_MLC.bin'];
        file_size =  39;
        fid = fopen(filename_ML,'r','s');
        dataML  = fread(fid, [file_size inf],'double');
        fclose(fid);

        % histogram data FPGA
        filename_FPGA=[pathData,'2kDMA_',FileDataNames{ChooseData(dataIdx)},'_LLC.bin'];
        file_size = 5;
        fid = fopen(filename_FPGA,'r','s');
        HistDataFPGA = fread(fid, [file_size inf],'double');
        fclose(fid);

        % histogram data FPGA
        filename_FPGA=[pathData,'2kDMA_',FileDataNames{ChooseData(dataIdx)},'_MLC.bin'];
        file_size = 5;
        fid = fopen(filename_FPGA,'r','s');
        HistDataML = fread(fid, [file_size inf],'double');
        fclose(fid);

        [HistDataRaw{1,dataIdx}.LLCL,HistDataRaw{1,dataIdx}.MLCL] = HistDataLabeling(HistDataFPGA,HistDataML); 
    end
    [DataRaw{1,dataIdx}.LLCL,DataRaw{1,dataIdx}.MLCL] = DataLabeling(dataFPGA,dataML);
end

%% Show data
dataIdx = 1;
figure, 
ax(1) = subplot(311);
hold on;
plot(DataRaw{1,dataIdx}.MLCL.LeftJointAngle); plot(DataRaw{1,dataIdx}.MLCL.RightJointAngle);...
    plot(DataRaw{1,dataIdx}.MLCL.AssistON*20); plot(DataRaw{1,dataIdx}.MLCL.SynchON*20);...
    legend('LX Angle (deg)', 'RX Angle (deg)', 'AssistON', 'SynchON')
ax(2) = subplot(312);
plot(DataRaw{1,dataIdx}.MLCL.Phase/max(DataRaw{1,dataIdx}.MLCL.Phase)*100); legend('Gait Phase (%)')
ax(3) = subplot(313);
plot(DataRaw{1,dataIdx}.MLCL.LeftActualTorque); legend('LX Joint Torque (Nm)')
linkaxes(ax,'x')
hold off;

%% 
% #1: Cut the data considering only the portion with the assistance
%     (AssistON == 1). Data of interest are: Joint Angle, Joint Torque,
%     Phase, Iter
% #2: Within this portion, segment data according to the gait cycle (use
%     the phase variable (the signal going from 0% to 100% at each step)
% #3: Identify the faster and slower steps (hint: you can calculate the
%     step duration based on the phase variable)
% #4: Plot the mean and std angle and torque profiles of at least 5 steps (both for
%     slower and faster steps)
% #5: Plot the histogram of the counter difference for LLCL and MLCL
%     considering the segmented steps


